#include "MiscFunctions.h"

#include <stdio.h>
#include <stdexcept>
#include <opencv2/core/core.hpp>

using namespace idch;
using namespace std;
using namespace cv;

void idch::findIntersection( const LineSegmentF &a ,
                       const LineSegmentF &b ,
                       float &pt_x , float &pt_y )
{
    float x1 = a.x0;
    float x2 = a.x1;
    float y1 = a.y0;
    float y2 = a.y1;
    float x3 = b.x0;
    float x4 = b.x1;
    float y3 = b.y0;
    float y4 = b.y1;


    float top = (x4-x3)*(y1 - y3) - (y4 - y3)*(x1 - x3);
    float bottom = (y4 - y3)*(x2 - x1) - (x4 - x3)*(y2 - y1);

    float u_a = top/bottom;

    pt_x = x1 + u_a *(x2 - x1);
    pt_y = y1 + u_a *(y2 - y1);

//    printf("interesection %f %f\n",pt_x,pt_y);
}

double idch::lineDistance2( const cv::Point &begin ,
                      const cv::Point &end ,
                      const cv::Point &p )
{
    double slopeX = end.x - begin.x;
    double slopeY = end.y - begin.y;

    return lineDistance2<double>(begin.x,begin.y,slopeX,slopeY,p.x,p.y);
}

double idch::acuteAngle( const LineSegmentF& a , const LineSegmentF& b )
{
    double dx0 = a.x1-a.x0; double dy0 = a.y1-a.y0;
    double dx1 = b.x1-b.x0; double dy1 = b.y1-b.y0;

    return acuteAngle<double>(dx0,dy0,dx1,dy1);
}

double idch::acuteAngle( const cv::Point2f &a , const cv::Point2f &b , const cv::Point2f &c ) {
    return acuteAngle<float>( b.x-a.x , b.y-a.y , c.x-a.x , c.y-a.y );
}

RectPt idch::convertRect( const LineSegRect& orig )
{
    RectPt ret;

    float x,y;

    findIntersection(orig.l,orig.t,x,y);
    ret.tl = cv::Point2f(x,y);
    findIntersection(orig.l,orig.b,x,y);
    ret.bl = cv::Point2f(x,y);
    findIntersection(orig.r,orig.t,x,y);
    ret.tr = cv::Point2f(x,y);
    findIntersection(orig.r,orig.b,x,y);
    ret.br = cv::Point2f(x,y);

//    printf("tl = %f %f  %f %f\n",ret.tl.x,ret.tl.y,x,y);

    return ret;
}

void idch::convertGray16to8( const cv::Mat& src , const cv::Mat& dst , uint16_t maxValue )
{
    if( src.depth() != 2 )
        throw std::runtime_error("src must have a depth of 2");

    if( src.channels() != 1 )
        throw std::runtime_error("src is expected to have one channel");

    const uint16_t *srcPtr = (const uint16_t *)src.datastart;
    const uint16_t *srcRowPtr = srcPtr;

    uint8_t *dstPtr = (uint8_t *)dst.datastart;
    uint8_t *dstRowPtr = dstPtr;

    int channels = dst.channels();

    for( int i = 0; i < src.rows; i++ ) {
        srcPtr = srcRowPtr;
        dstPtr = dstRowPtr;

        for( int j = 0; j < src.cols; j++ ) {

            uint8_t val = 0xFF * *srcPtr++ / maxValue;

            for( int k = 0; k < channels; k++ ) {
                *dstPtr++ = val;
            }
        }
        srcRowPtr += src.step/2;
        dstRowPtr += dst.step;
    }
}

void writeMatrix(FileStorage& output_file, const std::string& name, const cv::Mat& matrix)
{
  CvMat m = matrix;
  output_file.writeObj(name, &m);
}

void idch::saveCalibration( const StereoCalib& calib , int imgWidth , int imgHeight )
{
    FileStorage output_file ("kinect_calibration.yml",
                             CV_STORAGE_WRITE);
    writeMatrix(output_file, "rgb_intrinsics", calib.secondK);
    writeMatrix(output_file, "rgb_distortion", calib.secondCoef);
    writeMatrix(output_file, "depth_intrinsics", calib.firstK);
    writeMatrix(output_file, "depth_distortion", calib.firstCoef);
    writeMatrix(output_file, "R", calib.R);
    writeMatrix(output_file, "T", calib.T);
    cv::Mat1i size_matrix(1,2);
    size_matrix(0,0) = imgWidth;
    size_matrix(0,1) = imgHeight;
    writeMatrix(output_file, "rgb_size", size_matrix);
    writeMatrix(output_file, "raw_rgb_size", size_matrix);
    writeMatrix(output_file, "depth_size", size_matrix);
    writeMatrix(output_file, "raw_depth_size", size_matrix);
    output_file.release();
}
